/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.SpeedController;

public class PIDSpeedController implements SpeedController {
    private PIDController m_PIDController;
    private double m_maxSpeed;
    private double m_minSpeed;

    public PIDSpeedController(PIDController PIDControllerOutput, double maxSpeed, double minSpeed) {
        m_PIDController = PIDControllerOutput;
        m_maxSpeed = maxSpeed;
        m_minSpeed = minSpeed;
    }

    public double get() {
        return ((m_PIDController.getSetpoint() - m_minSpeed) / (m_maxSpeed - m_minSpeed)) * 2 - 1;
    }

    /**
     * Sets the speed of the PID loop.
     *
     * @deprecated Use set instead.
     * @param speed The speed to set the PID loop to.
     * @param syncGroup
     */
    public void set(double speed, byte syncGroup) {
        set(speed);
    }

    /**
     * Sets the speed of the PID loop.
     *
     * @param speed The speed to set the PID loop to.
     */
    public void set(double speed) {
        double setpoint = (speed * .5 + .5) * (m_maxSpeed - m_minSpeed) + m_minSpeed;
        m_PIDController.setSetpoint(setpoint);
    }

    /**
     * Disables the PID loop.
     */
    public void disable() {
        m_PIDController.disable();
    }

    /**
     * Same thing as set.
     * @param output The speed to set the output to.
     * @see set.
     */
    public void pidWrite(double output) {
        set(output);
    }

    public double getMaxSpeed() {
        return m_maxSpeed;
    }

    public double getMinSpeed() {
        return m_minSpeed;
    }

    public void setMaxSpeed(double maxSpeed) {
        m_maxSpeed = maxSpeed;
    }

    public void setMinSpeed(double minSpeed) {
        m_minSpeed = minSpeed;
    }

    public PIDController getPIDController() {
        return m_PIDController;
    }

    public void setPIDController(PIDController PIDControllerOutput) {
        m_PIDController = PIDControllerOutput;
    }
}